﻿#include "rams.h"
#include "ui_rams.h"

#include <QCoreApplication>
#include <sstream>
#include <iostream>
#pragma execution_character_set("utf-8")
static std::vector< TriadData > accData;
//static std::vector<TriadData> gyroData;
static CalibratedTriad device_acc_calib;
QMutex qmutex;
static void get_acc_data_onlion(mavlink_message_t &msg)
{
    double ts =  mavlink_msg_acc_data_get_time(&msg);
    //mavlink_msg_acc_cmd_get_cmd()
    double acc[3];
    mavlink_msg_acc_data_get_acc(&msg,acc);
//    qDebug()<<ts<<" "<< acc[0]<<" "<<acc[1]<<" "<<acc[2];
    qmutex.lock();
    accData.push_back ( TriadData(ts, acc[0],acc[1], acc[2]));
//    updata_acc();
    qmutex.unlock();
}
static void get_acc_cali_onlion(mavlink_message_t &msg)
{
    double mat[9];
    Vector3d bias;
    Matrix3d scale_mat;
    Matrix3d mis_mat;

    mavlink_msg_acc_calib_get_bias_vec(&msg,mat);
    bias = Eigen::Map< const Eigen::Matrix< double, 3, 1> >(mat);
    mavlink_msg_acc_calib_get_scale_mat(&msg,mat);
    scale_mat=Eigen::Map< const Eigen::Matrix< double, 3, 3, Eigen::RowMajor> >(mat);
    mavlink_msg_acc_calib_get_mis_mat(&msg,mat);
    mis_mat=Eigen::Map< const Eigen::Matrix< double, 3, 3, Eigen::RowMajor> >(mat);
    std::string filename ="device_acc.calib";
    std::ofstream file( filename.data() );
    if (file.is_open())
    {
        file<<mis_mat<<std::endl<<std::endl
            <<scale_mat<<std::endl<<std::endl
            <<bias<<std::endl<<std::endl;
    }
    cout<<mis_mat<<endl;
    file.close();
    device_acc_calib.load(filename);
}
static mavlink_message_t & tx_acc_cali_info(){
    mavlink_message_t msg;
    double mis_mat[9];
    double scale_mat[9];
    double bias_mat[3];
    for(int i = 0;i< 3;++i){
        for(int j = 0; j<3 ;++j){
            mis_mat[i*3+j] = device_acc_calib.getMisalignmentMatrix()(i,j);
            scale_mat[i*3+j] = device_acc_calib.getScaleMatrix()(i,j);
        }
        bias_mat[i] = device_acc_calib.getBiasVector()(i);
    }
    mavlink_msg_acc_calib_pack(1,1,&msg,0,mis_mat,scale_mat,bias_mat);
    return msg;
}
RAMS::RAMS(QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::RAMS)
{
    ui->setupUi(this);
    ui->lightButton->setGreen();
//    tcpClient->plainText=ui->plainTextEdit_info;
//    msg = new Msg;
    clientThread=new QThread(this);
    tcpClient = new TcpClient();
    /*************************************/
    // 回调函数
    tcpClient->acc_data_msg = get_acc_data_onlion;
    tcpClient->acc_cali_msg = get_acc_cali_onlion;
    tcpClient->tx_cali_msg = tx_acc_cali_info;
    /*************************************/
    tcpClient->moveToThread(clientThread);
    clientThread->start();
    connect(this,&RAMS::connectTcpServer,tcpClient,&TcpClient::connectToServer);
    connect(this,&RAMS::disconnectTcpServer,tcpClient,&TcpClient::disconnectServer);
    connect(tcpClient,&TcpClient::connected,this,[&](){
        ui->plainTextEdit_info->clear();
        ui->plainTextEdit_info->appendPlainText("Success");
        if(is_get_calib == false){
            // 自动获取标定值
            ui->pushButton_load->click();
        }
    });
    connect(tcpClient,&TcpClient::disconnected,this,[=](){
        ui->plainTextEdit_info->clear();
        ui->plainTextEdit_info->appendPlainText("与服务器断开连接");
    });
    connect(tcpClient,&QTcpSocket::readyRead,tcpClient,&TcpClient::receiveData);
    //
    connect(this,&RAMS::tcp_write,tcpClient,&TcpClient::accCmdWrite);
    qDebug()<<QThread::currentThreadId();

    connect(tcpClient,&TcpClient::received_cali,this,[=](){
        ui->plainTextEdit_acc_calib->clear();
        ui->plainTextEdit_acc_calib->insertPlainText(QString("Misalignment Matrix\n"));
        auto m = device_acc_calib.getMisalignmentMatrix();
        for(int i = 0 ; i<3 ;++i){
            ui->plainTextEdit_acc_calib->insertPlainText(QString("%1\t%2\t%3\n").arg(m(i,0)).arg(m(i,1)).arg(m(i,2)));
        }
        ui->plainTextEdit_acc_calib->insertPlainText(QString("Scale Matrix\n"));
        auto s = device_acc_calib.getScaleMatrix();
        for(int i = 0 ; i<3 ;++i){
            ui->plainTextEdit_acc_calib->insertPlainText(QString("%1\t%2\t%3\n").arg(s(i,0)).arg(s(i,1)).arg(s(i,2)));
        }
        ui->plainTextEdit_acc_calib->insertPlainText(QString("Bias Vector\n"));
        auto b = device_acc_calib.getBiasVector();
        ui->plainTextEdit_acc_calib->insertPlainText(QString("%1\t%2\t%3\n").arg(b[1]).arg(b[2]).arg(b[3]));

    });
    qmutex.lock();
    if(accData.empty())
    {
        accData.push_back ( TriadData(0, 0,0, 0));
    }
    qmutex.unlock();
    acc_data_it = 0;
    dataTimer = new QTimer(this);
    plot_init();
    // 定时器开始在 开始/关闭 数据接收的槽函数中
    connect(dataTimer,&QTimer::timeout,this,&RAMS::plot_update);

    /******IMU****/

    imu = new IMU();
    imu->set_acc_norm_threshold(1.005,0.995);

    /*************/

//    dataTimer->start(20);
//    get_data();
//    calib_acc();
}
RAMS::~RAMS()
{
    emit disconnectTcpServer();
    clientThread->requestInterruption();
    clientThread->quit();
    clientThread->exit();
    tcpClient->deleteLater();
    delete imu;
    delete ui;
}

void RAMS::plot_init()
{
    plot_acc(ui->qPlot_acc);
    plot_acc(ui->qPlot_acc_2);
    plot_angle(ui->qPlot_ang);
}
void RAMS::plot_update()
{
    if(accData.empty())
        return;
    qmutex.lock();
    for(/*int64_t it = acc_data_it*/; acc_data_it< accData.size()-1; ++acc_data_it)
    {
        auto a = accData[acc_data_it];
        ui->qPlot_acc->graph(0)->addData(a.timestamp(),(a.x()));
        ui->qPlot_acc->graph(1)->addData(a.timestamp(),(a.y()));
        ui->qPlot_acc->graph(2)->addData(a.timestamp(),(a.z()));

        ui->qPlot_acc_2->graph(0)->addData(a.timestamp(),(a.x()));
        ui->qPlot_acc_2->graph(1)->addData(a.timestamp(),(a.y()));
        ui->qPlot_acc_2->graph(2)->addData(a.timestamp(),(a.z()));

        // 添加ACC数据
        Eigen::Vector3d acc_affter_calib;
        acc_affter_calib = device_acc_calib.unbiasNormalize(a.data()); // 将加速度计数据校准
//        acc_affter_calib = a.data();
        IMU::STATE sta; // 判断是不是静止
        sta = imu->update_data(acc_affter_calib);

        ui->lineEdit_acc_n->setText(QString::number(acc_affter_calib.norm(),'f',3));

        ui->lineEdit_g_x->setText(QString::number(acc_affter_calib.x(),'f',3));
        ui->lineEdit_g_y->setText(QString::number(acc_affter_calib.y(),'f',3));
        ui->lineEdit_g_z->setText(QString::number(acc_affter_calib.z(),'f',3));

        if(sta == IMU::STATION){
            ui->lightButton->setGreen();
        }else{
            ui->lightButton->setRed();
        }

        if(show_angle){ // 是否开始测量。 由按键控制，实际上时是否显示和记录
            double tmp = imu->get_angle();
            angle_mean_filter.push_back(tmp);
            while(angle_mean_filter.size()>20){
                angle_mean_filter.pop_front();
            }
            double sum = 0;
            for_each(angle_mean_filter.begin(),angle_mean_filter.end(),[&](double ag){
                sum += ag;
            });
            AngleData ang;
            ang.angle = sum/angle_mean_filter.size();
            ang.time = a.timestamp();
            ui->qPlot_ang->graph(0)->addData(a.timestamp(),ang.angle);
            ui->gaugePanel->setValue(ang.angle);
//            qDebug()<<ang.angle;
            // 角度缓存
            angle_buf.push_back(ang);
        }
        //ui->qPlot_ang->graph(0)
    }
    qmutex.unlock();
    ui->qPlot_acc->xAxis->setRange(accData[acc_data_it].timestamp(), 10, Qt::AlignRight);
    ui->qPlot_acc->replot();
    ui->qPlot_acc_2->xAxis->setRange(accData[acc_data_it].timestamp(), 10, Qt::AlignRight);
    ui->qPlot_acc_2->replot();
    ui->qPlot_ang->xAxis->setRange(accData[acc_data_it].timestamp(), 10, Qt::AlignRight);
    ui->qPlot_ang->replot();

    //acc_data_it = accData.size()-1;
}

// 建立/取消 tcp 连接
void RAMS::on_switchButton_Socket_checkedChanged(bool checked)
{
    if(checked)
    {
        qDebug()<<ui->ipaddress->getIP()<<ui->lineEdit_Port->text().toInt();
//        tcpClient->connectToHost(QHostAddress(ui->ipaddress->getIP()),ui->lineEdit_Port->text().toInt());
        ui->plainTextEdit_info->clear();
        ui->plainTextEdit_info->insertPlainText("开始连接服务器！\n");
        emit connectTcpServer((ui->ipaddress->getIP()),ui->lineEdit_Port->text().toInt());
//        ui->comboBox_Mode->setEnabled(false);
//        clientTimerID = startTimer(600);
    }else {
//       tcpClient->close();
        emit disconnectTcpServer();
       ui->plainTextEdit_info->clear();
       ui->plainTextEdit_info->insertPlainText("与服务器断开连接！\n");
//       disconnect(tcpClient,&TcpClient::connected,tcpClient,&TcpClient::connectToServer);
//       disconnect(tcpClient,&QTcpSocket::readyRead,tcpClient,&TcpClient::receiveData);
    }
}
// 开始/关闭 加速度数据
void RAMS::on_switchButton_data_checkedChanged(bool checked)
{
    if(checked){
        emit tcp_write(START_TX);
        dataTimer->start(20);
    }else {
        emit tcp_write(STOP_TX);
        dataTimer->stop();
    }
}
// 从下位机载入校准数据
void RAMS::on_pushButton_load_clicked()
{
//    if(tcpCli)
    emit tcp_write(GET_CALIB);
    is_get_calib = true;
}
// 保存校准文件到下位机
void RAMS::on_pushButton_save_clicked()
{
    // SAVE;
    emit tcp_write(SAVE_CALIB);
}
// 缓存数据保存
void RAMS::on_pushButton_data_save_clicked()
{
    ui->switchButton_data->setChecked(false);
    QString fileName = QFileDialog::getSaveFileName(this,
                                                        "保存数据",
                                                        "",
                                                        "ASCII文件(*mat)");
    std::string filename;
    if(fileName.isNull()){
        filename ="RAMS_data1.mat";
    }else{
        //fileName += ".mat";
        filename = fileName.toStdString();
        if(filename.substr(filename.size()-4,4) != ".mat"){
            filename += ".mat";
        }
    }
    std::ofstream file( filename.data(),ios::trunc );
    if (file.is_open())
    {
        qmutex.lock();
        for(auto a:accData)
        {
            file<<a.timestamp()<<"\t"<<a.x()<<"\t"<<a.y()<<"\t"<<a.z()<<std::endl;
        }
        qmutex.unlock();
//        file<<mis_mat<<std::endl<<std::endl
//            <<scale_mat<<std::endl<<std::endl
//            <<bias<<std::endl<<std::endl;
    }
    file.close();
}
// 清除缓存数据
void RAMS::on_pushButton_data_clear_clicked()
{
    ui->switchButton_data->setChecked(false);

    acc_data_it = 0;
    qmutex.lock();
    accData.clear();
    qmutex.unlock();

    for(int i = 0; i<3; ++i)
    {
        ui->qPlot_acc->graph(i)->data().data()->clear();

        ui->qPlot_acc_2->graph(i)->data().data()->clear();
    }
    ui->qPlot_acc->replot();
    ui->qPlot_acc_2->replot();
//    plot_init();
}
// 开始校准
void RAMS::on_pushButton_cali_clicked()
{

    if(accData.size() > 1000)
    {
        qmutex.lock();
        device_acc_calib.setBias( Vector3d(0, 0, 0) );
        device_acc_calib.setScale(Vector3d(1,1,1));
        //get_data();
        calib_acc(device_acc_calib,accData,*ui->plainTextEdit_acc_calib);
        qmutex.unlock();

    }else{
        QMessageBox::information(this, QString("Warning"),QString("在线数据太少"));

        QString fileName = QFileDialog::getOpenFileName(this,
                                                            "选择本地数据",
                                                            "",
                                                            "ASCII文件(*mat);;"
                                                               "本本文件(*txt)");
        if(!fileName.isNull()){
            qDebug()<<"filenames : "<<fileName;
            get_data(fileName);
            calib_acc(device_acc_calib,acc_data,*ui->plainTextEdit_acc_calib);
        }
    }
    device_acc_calib.save("device_acc.calib");
}
// 载入测试数据
void RAMS::on_pushButton_load_test_clicked()
{
    QString fileName = QFileDialog::getOpenFileName(this,
                                                        "选择本地数据",
                                                        "",
                                                        "ASCII文件(*mat);;"
                                                           "本本文件(*txt)");
    if(!fileName.isNull()){
        qDebug()<<"filenames : "<<fileName;
        std::string str = fileName.toStdString();
        const char* x_acc = str.c_str();
        imu->importAsciiData(x_acc);
    }
}

void RAMS::get_data(QString s)
{
    std::string str = s.toStdString();
    const char* x_acc = str.c_str();
    acc_data.clear();
    importAsciiData( x_acc, acc_data, imu_tk::TIMESTAMP_UNIT_SEC );
}
// 计算旋转轴
void RAMS::on_pushButton_cal_axis_clicked()
{
//    ui->lightButton->setGreen();
    qDebug()<<"计算旋转轴\n";
    if(imu->is_rotation_axis_calculated()){
        QMessageBox::StandardButton result;//返回选择的按钮
        result=QMessageBox::question(this, "旋转轴已存在", "是否重新计算旋转轴，若是请旋转活动面三次后重新点击该按钮",
                          QMessageBox::Yes|QMessageBox::No);
        if(result == QMessageBox::Yes){
            imu->reset_rotation_axis();
        }
    }else{
        QString res = imu->calculate_rotation_axis();
        if(res == "Success!!"){
            Eigen::Vector3d axis =  imu->get_rotation_axis();
            QMessageBox::information(this, QString("Good!"),res);
            std::cout<<"rotation axis:"<<axis;

            ui->lineEdit_ra_x->setText(QString::number(axis.x(),'f',3));
            ui->lineEdit_ra_y->setText(QString::number(axis.y(),'f',3));
            ui->lineEdit_ra_z->setText(QString::number(axis.z(),'f',3));
        }else{
            ui->lineEdit_ra_x->setText(QString::number(1));
            ui->lineEdit_ra_y->setText(QString::number(0));
            ui->lineEdit_ra_z->setText(QString::number(0));
            QMessageBox::information(this, QString("Error"),res);
        }
    }
//    ui->lightButton->setRed();
}
// 设定旋转起点
void RAMS::on_pushButton_first_acc_clicked()
{
    QString res;
    res = imu->set_rotation_starting_point();
    if(imu->is_init_first_acc()){
        Eigen::Vector3d a = imu->get_first_acc();
        ui->lineEdit_acc_x->setText(QString::number(a.x(),'f',3));
        ui->lineEdit_acc_y->setText(QString::number(a.y(),'f',3));
        ui->lineEdit_acc_z->setText(QString::number(a.z(),'f',3));
    }else{
        ui->lineEdit_acc_x->setText(QString::number(0));
        ui->lineEdit_acc_y->setText(QString::number(0));
        ui->lineEdit_acc_z->setText(QString::number(1));
    }
    QMessageBox::information(this, QString("Info"),res);
}

void RAMS::on_pushButton_start_clicked()
{

    if(ui->pushButton_start->text() == "开始测量"){
        if(imu->is_init_first_acc() && imu->is_rotation_axis_calculated()){
            show_angle = true;
            ui->pushButton_start->setText("停止测量");
        }else{
            QMessageBox::information(this, QString("Info"),QString("请先设定旋转轴和起点"));
        }
    }else{
        show_angle = false;
        ui->pushButton_start->setText("开始测量");
    }
}

void RAMS::on_pushButton_sv_angle_clicked()
{
    ui->switchButton_data->setChecked(false);
    if(angle_buf.size()<10){
        QMessageBox::information(this, QString("Info"),QString("测量结果太少"));
    }else{
        QString fileName = QFileDialog::getSaveFileName(this,
                                                            "保存测量数据",
                                                            "",
                                                            "ASCII文件(*mat)");
        std::string filename;
        if(fileName.isNull()){
            filename ="RAMS_AngleData.mat";
        }else{
            //fileName += ".mat";
            filename = fileName.toStdString();
            if(filename.substr(filename.size()-4,4) != ".mat"){
                filename += ".mat";
            }
        }
        std::ofstream file( filename.data(),ios::trunc );
        if (file.is_open())
        {
            for(auto a:angle_buf)
            {
                file<<a.time<<"\t"<<a.angle<<std::endl;
            }
        }
        file.close();
    }
}

void RAMS::plot_angle(QCustomPlot *qPlot)
{
    QCPTextElement *m_title;
    m_title = new QCPTextElement(qPlot, "Rotation angle");
    m_title->setFont(QFont("sans",10,QFont::Bold));
    qPlot->plotLayout()->insertRow(0);
    qPlot->plotLayout()->addElement(0, 0, m_title);

    qPlot->xAxis2->setVisible(true);
    qPlot->xAxis2->setTickLabels(false);
    qPlot->yAxis2->setVisible(true);
    qPlot->yAxis2->setTickLabels(false);
    connect(qPlot->xAxis, SIGNAL(rangeChanged(QCPRange)), qPlot->xAxis2, SLOT(setRange(QCPRange)));
    connect(qPlot->yAxis, SIGNAL(rangeChanged(QCPRange)), qPlot->yAxis2, SLOT(setRange(QCPRange)));

    qPlot->setLocale(QLocale(QLocale::English, QLocale::UnitedKingdom));
    qPlot->addGraph();
    QPen pen = QPen(QColor(Qt::red));
    pen.setWidth(2);
    qPlot->graph(0)->setPen(pen);
    qPlot->graph(0)->setName("angle");

    qPlot->yAxis->setRange(-180, 180);
    QSharedPointer<QCPAxisTickerTime> timeTicker(new QCPAxisTickerTime);
    timeTicker->setTimeFormat("%h:%m:%s");
    qPlot->xAxis->setTicker(timeTicker);
//    qPlot->xAxis->setRange(angle_data[angle_data.size()-1].timestamp(), 20, Qt::AlignRight);

    qPlot->legend->setVisible(true);
    qPlot->legend->setFont(QFont("Helvetica",10));
//    qPlot->plotLayout()->insertRow(1);
//    qPlot->plotLayout()->addElement(1, 0, qPlot->legend);
//    qPlot->plotLayout()->setRowStretchFactor(1, 0.001);
//    qPlot->legend->addElement(0,0,qPlot->legend->item(0));


//    for(auto a:angle_data)
//    {
//        qPlot->graph(0)->addData(a.timestamp(),(a.x()));
//        qPlot->graph(1)->addData(a.timestamp(),(a.y()));
//        qPlot->graph(2)->addData(a.timestamp(),(a.z()));
//    }
    //qPlot->xAxis->setRange(acc_data[acc_data.size()-1].timestamp(), 8, Qt::AlignRight);
    qPlot->setInteractions(QCP::iRangeDrag | QCP::iRangeZoom | QCP::iSelectPlottables);
}

void RAMS::plot_acc(QCustomPlot *qPlot)
{
//    QColor color(Qt::lightGray);
//    QBrush brush(color);
//    qPlot->setBackground(brush);

    QCPTextElement *m_title;
    m_title = new QCPTextElement(qPlot, "ACC");
    m_title->setFont(QFont("sans",10,QFont::Bold));
    qPlot->plotLayout()->insertRow(0);
    qPlot->plotLayout()->addElement(0, 0, m_title);

    qPlot->xAxis2->setVisible(true);
    qPlot->xAxis2->setTickLabels(false);
    qPlot->yAxis2->setVisible(true);
    qPlot->yAxis2->setTickLabels(false);
    connect(qPlot->xAxis, SIGNAL(rangeChanged(QCPRange)), qPlot->xAxis2, SLOT(setRange(QCPRange)));
    connect(qPlot->yAxis, SIGNAL(rangeChanged(QCPRange)), qPlot->yAxis2, SLOT(setRange(QCPRange)));

    qPlot->setLocale(QLocale(QLocale::English, QLocale::UnitedKingdom));
    qPlot->addGraph();
    QPen pen = QPen(QColor(Qt::red));
    pen.setWidth(2);
    qPlot->graph(0)->setPen(pen);
    qPlot->graph(0)->setName("acc_x");
    qPlot->addGraph();
    QPen pen1 = QPen(QColor(Qt::blue));
    pen1.setWidth(2);
    qPlot->graph(1)->setPen(pen1);
    qPlot->graph(1)->setName("acc_y");
    qPlot->addGraph();
    QPen pen2 = QPen(QColor(Qt::magenta));
    pen2.setWidth(2);
    qPlot->graph(2)->setPen(pen2);
    qPlot->graph(2)->setName("acc_z");

    qPlot->yAxis->setRange(-2, 2);
    QSharedPointer<QCPAxisTickerTime> timeTicker(new QCPAxisTickerTime);
    timeTicker->setTimeFormat("%h:%m:%s");
    qPlot->xAxis->setTicker(timeTicker);
    //qPlot->xAxis->setRange(acc_data[acc_data.size()-1].timestamp(), 20, Qt::AlignRight);

    qPlot->legend->setVisible(true);
    qPlot->legend->setFont(QFont("Helvetica",10));
    qPlot->plotLayout()->insertRow(1);
    qPlot->plotLayout()->addElement(1, 0, qPlot->legend);
    qPlot->plotLayout()->setRowStretchFactor(1, 0.001);
    qPlot->legend->addElement(0,0,qPlot->legend->item(0));
    qPlot->legend->addElement(0,1,qPlot->legend->item(1));
    qPlot->legend->addElement(0,2,qPlot->legend->item(2));
//    for(auto a:acc_data)
//    {
////        qPlot->graph(0)->addData(a.timestamp(),(a.x()-33124)*0.00241278);
////        qPlot->graph(1)->addData(a.timestamp(),(a.y()-33124)*0.00241278);
////        qPlot->graph(2)->addData(a.timestamp(),(a.z()-33124)*0.00241278);
//        qPlot->graph(0)->addData(a.timestamp(),(a.x()));
//        qPlot->graph(1)->addData(a.timestamp(),(a.y()));
//        qPlot->graph(2)->addData(a.timestamp(),(a.z()));
//    }
    //qPlot->xAxis->setRange(acc_data[acc_data.size()-1].timestamp(), 8, Qt::AlignRight);
    qPlot->setInteractions(QCP::iRangeDrag | QCP::iRangeZoom | QCP::iSelectPlottables);
}

void RAMS::get_data()
{
//    dataTimer = new QTimer(this);
//    const char x_acc[] = "D://Code//C++//imu_tk//bin//test_data//xsens_acc.mat";
    const char x_acc[] = "RAMS_data.mat";
    const char x_gyro[] = "D://Code//C++//imu_tk//bin//test_data//xsens_gyro.mat";
//    const char x_acc[] = "D://Code//C++//imu_tk//bin//test_data//acc.mat";
//    const char x_gyro[] = "D://Code//C++//imu_tk//bin//test_data//gyro.mat";
//    const char m_angle[] = "D://Code//C++//imu_tk//bin//test_data//angle.mat";
    importAsciiData( x_acc, acc_data, imu_tk::TIMESTAMP_UNIT_SEC );
    importAsciiData( x_gyro, gyro_data, imu_tk::TIMESTAMP_UNIT_SEC );
//    importAsciiData( m_angle, angle_data, imu_tk::TIMESTAMP_UNIT_MSEC );
//    cout<<acc_data.size()<<endl;
//    cout<<acc_data[1000].timestamp()<<endl;
//    cout<<acc_data[1001].timestamp()<<endl;
//    cout<<acc_data[0].timestamp()<<endl;
//    plot_acc(ui->qPlot_acc);
//    plot_gyro(ui->qPlot_gyro);
//    plot_acc(ui->qPlot_acc_2);
//    plot_gyro(ui->qPlot_gyro_2);
//    plot_angle(ui->qPlot_ang);

//    ui->gaugePanel->setValue(angle_data[angle_data.size()-1].x());
}

void RAMS::plot_gyro(QCustomPlot *qPlot)
{
    QCPTextElement *m_title;
    m_title = new QCPTextElement(qPlot, "GYRO");
    m_title->setFont(QFont("sans",12,QFont::Bold));
    qPlot->plotLayout()->insertRow(0);
    qPlot->plotLayout()->addElement(0, 0, m_title);

    qPlot->xAxis2->setVisible(true);
    qPlot->xAxis2->setTickLabels(false);
    qPlot->yAxis2->setVisible(true);
    qPlot->yAxis2->setTickLabels(false);
    connect(qPlot->xAxis, SIGNAL(rangeChanged(QCPRange)), qPlot->xAxis2, SLOT(setRange(QCPRange)));
    connect(qPlot->yAxis, SIGNAL(rangeChanged(QCPRange)), qPlot->yAxis2, SLOT(setRange(QCPRange)));
    qPlot->legend->setVisible(true);
    qPlot->legend->setFont(QFont("Helvetica",10));

    qPlot->setLocale(QLocale(QLocale::English, QLocale::UnitedKingdom));
    qPlot->addGraph();
    qPlot->graph(0)->setPen(QPen(QColor(Qt::red)));
    qPlot->graph(0)->setName("gyro_x");
    qPlot->addGraph();
    qPlot->graph(1)->setPen(QPen(QColor(Qt::blue)));
    qPlot->graph(1)->setName("gyro_y");
    qPlot->addGraph();
    qPlot->graph(2)->setPen(QPen(QColor(Qt::magenta)));
    qPlot->graph(2)->setName("gyro_z");
    qPlot->yAxis->setRange(-30, 30);
    QSharedPointer<QCPAxisTickerTime> timeTicker(new QCPAxisTickerTime);
    timeTicker->setTimeFormat("%h:%m:%s");
    qPlot->xAxis->setTicker(timeTicker);
//     qPlot->xAxis->setRange(0, 40);
      qPlot->xAxis->setRange(gyro_data[gyro_data.size()-1].timestamp(), 20, Qt::AlignRight);
    for(auto a:gyro_data)
    {
//        qPlot->graph(0)->addData(a.timestamp(),(a.x()-33124)*0.00241278);
//        qPlot->graph(1)->addData(a.timestamp(),(a.y()-33124)*0.00241278);
//        qPlot->graph(2)->addData(a.timestamp(),(a.z()-33124)*0.00241278);
        qPlot->graph(0)->addData(a.timestamp(),(a.x()));
        qPlot->graph(1)->addData(a.timestamp(),(a.y()));
        qPlot->graph(2)->addData(a.timestamp(),(a.z()));
    }

    qPlot->legend->setVisible(true);
    qPlot->legend->setFont(QFont("Helvetica",10));
    qPlot->plotLayout()->insertRow(1);
    qPlot->plotLayout()->addElement(1, 0, qPlot->legend);
    qPlot->plotLayout()->setRowStretchFactor(1, 0.001);
    qPlot->legend->addElement(0,0,qPlot->legend->item(0));
    qPlot->legend->addElement(0,1,qPlot->legend->item(1));
    qPlot->legend->addElement(0,2,qPlot->legend->item(2));

    //qPlot->xAxis->setRange(acc_data[acc_data.size()-1].timestamp(), 8, Qt::AlignRight);
    qPlot->setInteractions(QCP::iRangeDrag | QCP::iRangeZoom | QCP::iSelectPlottables);
}

void RAMS::calib_acc(CalibratedTriad& init_acc_calib,vector< TriadData > &acc,QPlainTextEdit& plainTextEdit )
{
    //CalibratedTriad init_acc_calib;
    qDebug() << acc.size();
    MultiPosCalibration mp_calib;
    mp_calib.setInitStaticIntervalDuration(10.0); // 开始静止的时间 很重要
    mp_calib.setInitAccCalibration( init_acc_calib );
    mp_calib.setGravityMagnitude(1);
    mp_calib.enableVerboseOutput(true);
    mp_calib.enableAccUseMeans(false);
    mp_calib.calibrateAcc(acc,plainTextEdit);
    init_acc_calib = mp_calib.getAccCalib();
}
void RAMS::calib_acc()
{
    vector< TriadData > acc_data, gyro_data;
    //"D://Code//C++//imu_tk//bin//test_data//xsens_acc.mat"
    //"D://Code//C++//imu_tk//bin//test_data//xsens_gyro.mat"
    const char x_acc[] = "D://Code//C++//imu_tk//bin//test_data//xsens_acc.mat";
    const char x_gyro[] = "D://Code//C++//imu_tk//bin//test_data//xsens_gyro.mat";
    cout<<"Importing IMU data from the Matlab matrix file : "<< x_acc<<endl;
    importAsciiData( x_acc, acc_data, imu_tk::TIMESTAMP_UNIT_SEC );
    cout<<"Importing IMU data from the Matlab matrix file : "<< x_gyro<<endl;
    importAsciiData( x_gyro, gyro_data, imu_tk::TIMESTAMP_UNIT_SEC  );


    CalibratedTriad init_acc_calib, init_gyro_calib;
    init_acc_calib.setBias( Vector3d(32768, 32768, 32768) );
    init_gyro_calib.setScale( Vector3d(1.0/6258.0, 1.0/6258.0, 1.0/6258.0) );

    MultiPosCalibration mp_calib;

    mp_calib.setInitStaticIntervalDuration(50.0);
    mp_calib.setInitAccCalibration( init_acc_calib );
    mp_calib.setInitGyroCalibration( init_gyro_calib );
    mp_calib.setGravityMagnitude(9.81744);
    mp_calib.enableVerboseOutput(true);
    mp_calib.enableAccUseMeans(false);
    //mp_calib.setGyroDataPeriod(0.01);
    mp_calib.calibrateAcc(acc_data);
    //mp_calib.calibrateAccGyro(acc_data, gyro_data );
    mp_calib.getAccCalib().save("test_imu_acc.calib");
   // mp_calib.getGyroCalib().save("test_imu_gyro.calib");
}




